/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#ifndef CAR_H
#define CAR_H

#include "Object.h"
#include "Shapes.h"
#include "gimi.h"
#include "../MaCI/interfaces/SpeedCtrl/SpeedCtrlServer.hpp"
#include "../MaCI/interfaces/Position/PositionServer.hpp"
#include "Camera.h"
#include "Car_settings.h"

class CCar : public CObject
{
  public:
    CCar(CWorld* World, float x, float y, float heading, std::string name,
         std::string MaCIGroup, bool driverCam=false, bool headCam=false,
         bool boomCam=false, float boomFOV=BOOM_CAMERA_FOV, float boomLength=BOOM_LENGTH, 
         float boomHeight=BOOM_CAMERA_HEIGHT);
    ~CCar();

    void Activate();

    void Update(float StepSize);

    void OnCollision(CPrimitive *primitive, int numc, dContact *contacts, 
                     CPrimitive *primitive2);

    void Draw();

    static CObject* LoadMLMA(CWorld *world, xmlNode *node);
    
  private:
    
    std::string name;
    std::string MaCIGroup;
    
    gimi::GIMI gimi;
    
    MaCI::MaCICtrl::CMaCICtrlServer mcs;
    MaCI::SpeedCtrl::CSpeedCtrlServer SpeedCtrlServer;
    MaCI::SpeedCtrl::CSpeedCtrlData SpeedCtrlData;
    MaCI::Position::CPositionServer PositionServer;
    
    CBox *body, *roof;
    CBox *boom;
    CBox *frontRightBumper, *frontLeftBumper;
    CBox *rearRightBumper, *rearLeftBumper;
    CBox *crossBlock, *topBlock;
    CCylinder *frontRightWheel, *frontLeftWheel;
    CCylinder *rearRightWheel, *rearLeftWheel;
    
    CCamera *boomCamera, *driverCamera, *headCamera;
    
    TMaterial wheelMaterial, bodyMaterial, boomMaterial;
    
    
    float boomLength, boomHeight;
    dJointID boomJoint;
    dJointID frontRightJoint, frontLeftJoint;
    dJointID rearRightJoint, rearLeftJoint;
    
    float speed; //speed in m/s
    float posX, posY, posZ, heading;
    
    float positionTimer;
    float speedTimer;
    
    void SpeedCtrl();
    
    void steer(float angle);
    
    //power from 0 to 1;
    void throttle(float power);
    void reverse(float power);
    void brake(float power);
    
    void idle();
    
    void connectGIMI();
    void setPose(float x, float y, float heading);
};


#endif
